

//#include "minmea.h"
#include <math.h>
#include "common_api.h"
#include "luat_rtos.h"
#include "luat_debug.h"
#include "luat_uart.h"
#include "luat_pm.h"
//#include "agnss.h"
//#include "usr_gnss.h"
#include "main.h"
#include "luat_mobile.h"

#include "luat_network_adapter.h"


//#include "lbsLoc.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include "luat_fs.h"
//#include //"agnss.h"
#include "luat_rtc.h"
#include "luat_http.h"
#include "netdb.h"
#include "sockets.h"
#include "dns.h"
#include "lwip/ip4_addr.h"
luat_rtos_task_handle bsloc_task_handle;
#define pi 3.1415926
#define DEMO_SERVER_UDP_IP "bs.openluat.com" // 基站定位网址
#define DEMO_SERVER_UDP_PORT 12411           // 端口
#define UART_ID 2
#define USE_HD8128
/// @brief 合宙IOT 项目productkey ，必须加上，否则定位失败
static char *productKey = "XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX";
static const uint8_t HD8128_UART0_115200_CMD[] = {0xf1,0xd9,0x06,0x00,0x08,0x00,0x00,0x00,0x00,0x00,0x00,0xc2,0x01,0x00,0xd1,0xe0};
typedef struct
{
    char *gnss_data;
    size_t gnss_data_len;
}gnss_data_struct;


enum
{
	EVENT_NEW_GNSS_DATA = 1,
	EVENT_CHANGE_BR,
    EVENT_NEW_GNSS_DATA_PARSE,
};
/******************************************************************************/
/* 输入： 起始点经纬度(lng1, lat1),终止点经纬度(lng2, lat2)                   */
/* 输出： 起始点到终止点距离 result[0],起始点与终止点夹角 result[1]           */
/* 说明： 正北方向为0°，顺时针旋转增加，采用弧度制                           */
/******************************************************************************/
double gps_cacl(double lng1, double lat1, double lng2, double lat2)
{
    double a = 6378137.0;        // 椭球最大半径（地心到赤道）
    double b = 6356752.31425;    // 椭球最小半径（地心到两极）
    double f = (a - b) / a;      // 椭球扁率(flattening)
    double fai1 = lat1 * pi / 180;
    double fai2 = lat2 * pi / 180;
    double L = (lng2 - lng1) * pi / 180;
 
    double tanU1 = (1 - f) * tan(fai1);
    double cosU1 = 1 / sqrt(1 + tanU1 * tanU1);
    double sinU1 = tanU1 * cosU1;
 
    double tanU2 = (1 - f) * tan(fai2);
    double cosU2 = 1 / sqrt(1 + tanU2 * tanU2);
    double sinU2 = tanU2 * cosU2;
 
    double lamda = L;
    double sin_lamda;
    double cos_lamda;
    double sinSq_sigma;
    double sin_sigma;
    double cos_sigma = 0;
    double sigma = 0;
    double sin_alpha;
    double cosSq_alpha = 0;
    double cos_2sigmaM = 0;
    double C;
    double lamda_2;
    int k;
    for (k = 0; k < 100; k++)
    {
        sin_lamda = sin(lamda);
        cos_lamda = cos(lamda);
        sinSq_sigma = (cosU2 * sin_lamda) * (cosU2 * sin_lamda) + (cosU1 * sinU2 - sinU1 * cosU2 * cos_lamda) * (cosU1 * sinU2 - sinU1 * cosU2 * cos_lamda);
        sin_sigma = sqrt(sinSq_sigma);
        if (sin_sigma == 0)
            break;
        cos_sigma = sinU1 * sinU2 + cosU1 * cosU2 * cos_lamda;
        sigma = atan2l(sin_sigma, cos_sigma);
        sin_alpha = cosU1 * cosU2 * sin_lamda / sin_sigma;
        cosSq_alpha = 1 - sin_alpha * sin_alpha;
        cos_2sigmaM = cos_sigma - 2 * sinU1 * sinU2 / cosSq_alpha;
        if (isnan(cos_2sigmaM))
            cos_2sigmaM = 0;
        C = f / 16 * cosSq_alpha * (4 + f * (4 - 3 * cosSq_alpha));
        lamda_2 = lamda;
        lamda = L + (1 - C) * f * sin_alpha * (sigma + C * sin_sigma * (cos_2sigmaM + C * cos_sigma * (-1 + 2 * cos_2sigmaM * cos_2sigmaM)));
        if (abs(lamda - lamda_2) < 1e-12)
            break;
    }
    double uSq = cosSq_alpha * (a * a - b * b) / (b * b);
    double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
    double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
    double delta_sigma = B * sin_sigma * (cos_2sigmaM + B / 4 * (cos_sigma * (-1 + 2 * cos_2sigmaM * cos_2sigmaM) - B / 6 * cos_2sigmaM * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * cos_2sigmaM *cos_2sigmaM)));
    double result= b * A * (sigma - delta_sigma);// 最终距离
    return result;
}
/**
 * @brief Get the gnss lat object
 *
 * @param dev
 * @return int
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
int get_gnss_lat(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    int lat = dev->lat;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return lat;
}

/**
 * @brief Get the gnss lon object
 *
 * @param dev
 * @return int
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
int get_gnss_lon(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    int lon = dev->lon;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return lon;
}

/**
 * @brief Get the gnss sv object
 *
 * @param dev
 * @return int
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
int get_gnss_sv(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    int sv = dev->sv;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return sv;
}

/**
 * @brief Get the gnss speed object
 *
 * @param dev
 * @return int
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
float get_gnss_speed(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    float speed = dev->spd;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return speed;
}

/**
 * @brief Get the gnss ts object
 *
 * @param dev
 * @return time_t
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
time_t get_gnss_ts(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    time_t ts = dev->ts;
    if (ts < 1577808000)
        ts = 0;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return ts;
}
/**
 * @brief Get the gnss csq object
 *
 * @param dev
 * @return uint8_t
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
uint8_t get_gnss_csq(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    uint8_t csq = dev->csq;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return csq;
}

/**
 * @brief Get the gnss fixMode object
 *
 * @param dev
 * @return uint8_t
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
uint8_t get_gnss_fixMode(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    uint8_t fixMode = dev->fixMode;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return fixMode;
}

uint8_t get_gnss_gps_status(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    uint8_t gps_stauts=0;
    if (dev->gps_status=="A")
    {
        gps_stauts = 1;
    }
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return gps_stauts;
}

/**
 * @brief Get the gnss ns object
 *
 * @param dev
 * @return char*
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
float get_gnss_vt(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    float du = dev->vt;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return du;
}

/**
 * @brief Get the gnss ew object
 *
 * @param dev
 * @return char*
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */


double get_accumulated_mileage(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    double mileage = dev->Accumulated_mileage;   
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return mileage;
}
double get_mileage(DEV_GNSS_T *dev)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    double mileage = dev->mileage;   
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return mileage;
}
/**
 * @brief Get the bsloc lat object
 *
 * @param bl
 * @return int
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
int get_bsloc_lat(BASE_LOC_T *bl)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    int lat = bl->latitude;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return lat;
}

/**
 * @brief Get the bsloc lon object
 *
 * @param bl
 * @return int
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
int get_bsloc_lon(BASE_LOC_T *bl)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    int lon = bl->longitude;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return lon;
}

/**
 * @brief Get the bsloc ts object
 *
 * @param bl
 * @return time_t
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
time_t get_bsloc_ts(BASE_LOC_T *bl)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    time_t ts = bl->ts;
    if (ts < 1577808000)
        ts = 0;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return ts;
}

/**
 * @brief Get the bsloc state object
 *
 * @param bl
 * @return BSLOC_STATE_E
 * @author zcj (605607217@qq.com)
 * @date 2023-11-06
 */
BSLOC_STATE_E get_bsloc_state(BASE_LOC_T *bl)
{
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    BSLOC_STATE_E state = bl->result;
    luat_rtos_mutex_unlock(g_ebike_status.mutex);
    return state;
}

luat_rtos_task_handle gnss_in_task_handle;
static uint8_t gnss_work_mode;
static uint8_t gnss_uart_error_cnt;
static int libminmea_parse_data(const char *data, size_t len)
{
    size_t prev = 0;
    DEV_GNSS_T *gnss = (DEV_GNSS_T *)malloc(sizeof(DEV_GNSS_T));
    memset(gnss, 0, sizeof(DEV_GNSS_T));
    static char nmea_tmp_buff[86] = {0}; // nmea 最大长度82,含换行符
    //LUAT_DEBUG_PRINT("gnssdata:%.*s", len, data);
    for (size_t offset = 0; offset < len; offset++)
    {
        // \r == 0x0D  \n == 0x0A
        //LUAT_DEBUG_PRINT("gnssdata:%.*s", offset, data);
        if (data[offset] == 0x0A)
        {
            // 最短也需要是 OK\r\n
            // 应该\r\n的
            // 太长了
            //LUAT_DEBUG_PRINT("gnssdata:%.*s", offset, data);
            if (offset - prev < 3 || data[offset - 1] != 0x0D || offset - prev > 82)
            {
                prev = offset + 1;
                continue;
            }
            memcpy(nmea_tmp_buff, data + prev, offset - prev - 1);
            nmea_tmp_buff[offset - prev - 1] = 0x00;
            //LUAT_DEBUG_PRINT("gnssdata:%.*s", offset - prev - 1, nmea_tmp_buff);
            if (strstr(nmea_tmp_buff, "GNRMC"))
            {
                //usr_log(">>>>>>gnss: IS GNRMC\r\n");
                parse_nmea((const char *)nmea_tmp_buff, gnss);
               
            }
            else if (strstr(nmea_tmp_buff, "GNGGA"))
            {
               // usr_log(">>>>>>gnss: IS GNGGA\r\n");
                parse_nmea((const char *)nmea_tmp_buff, gnss);
            }
            prev = offset + 1;
        }
    }
    if (luat_rtos_message_send(gps_event_deal_task_handle, 0, (DEV_GNSS_T *)gnss) != 0)
    {
        free(gnss);
    }
    return 0;
}



int parse_nmea(const char *gnssdata, DEV_GNSS_T *usr_gnss)
{
    switch (minmea_sentence_id(gnssdata, false))
    {
        case MINMEA_SENTENCE_RMC:
        {
            struct minmea_sentence_rmc frame;
            if (minmea_parse_rmc(&frame, gnssdata))
            {
                usr_gnss->lat = minmea_tocoord(&frame.latitude) * 1000000;
                usr_gnss->lon = minmea_tocoord(&frame.longitude) * 1000000;
                usr_gnss->spd = frame.speed.value/ 1000;
                if (frame.valid)
                {
                    usr_gnss->gps_status = "A";
                }
                else
                {
                    usr_gnss->gps_status = "V";
                }
                
                usr_gnss->vt= frame.course.value/1000;
                struct timespec ts;
                minmea_gettime(&ts, &frame.date, &frame.time);
                usr_gnss->ts = ts.tv_sec;
            }     
        }
        break;
        case MINMEA_SENTENCE_GGA:
        {
            struct minmea_sentence_gga frame;
            if (minmea_parse_gga(&frame, gnssdata))
            {
                // if (frame.fix_quality)
                // {
                //     usr_gnss->fixMode = 3;
                // }
                // else
                // {
                //     usr_gnss->fixMode = 0;
                // }
                usr_gnss->fixMode =frame.fix_quality;
                usr_gnss->sv = frame.satellites_tracked;
            }
        }
        break;
    }
    return 0;
}
void gnss_in_uart_recv_cb(int uart_id, uint32_t data_len)
{
    char *data_buff = malloc(data_len + 1);
    memset(data_buff, 0, data_len + 1);
    luat_uart_read(uart_id, data_buff, data_len);
    gnss_data_struct *gnss = malloc(sizeof(gnss_data_struct));
    gnss->gnss_data = data_buff;
    gnss->gnss_data_len = data_len;
    if (luat_rtos_message_send(gnss_in_task_handle, 0, (void *)gnss) != 0)
    {
        free(gnss->gnss_data);
        free(gnss);
    }
}
/// @brief 把string 转换为BCD 编码
/// @param arr 字符串输入
/// @param len 长度
/// @param outPut 输出
/// @return
static uint8_t imeiToBcd(uint8_t *arr, uint8_t len, uint8_t *outPut)
{
    if (len % 2 != 0)
    {
        arr[len] = 0x0f;
    }

    uint8_t tmp = 0;

    for (uint8_t j = 0; j < len; j = j + 2)
    {
        outPut[tmp] = (arr[j] & 0x0f) << 4 | (arr[j + 1] & 0x0f);
        tmp++;
    }
    for (uint8_t i = 0; i < 8; i++)
    {
        outPut[i] = (outPut[i] % 0x10) * 0x10 + (outPut[i] - (outPut[i] % 0x10)) / 0x10;
    }
    return 0;
}
/// @brief BCD ->> str
/// @param pOutBuffer
/// @param pInBuffer
/// @param nInLen 长度
/// @return
uint32_t location_service_bcd_to_str(uint8_t *pOutBuffer, uint8_t *pInBuffer, uint32_t nInLen)
{
    uint32_t len = 0;
    uint8_t ch;
    uint8_t *p = pOutBuffer;
    uint32_t i = 0;

    if (pOutBuffer == NULL || pInBuffer == NULL || nInLen == 0)
    {
        return 0;
    }

    for (i = 0; i < nInLen; i++)
    {
        ch = pInBuffer[i] & 0x0F;
        if (ch == 0x0F)
        {
            break;
        }
        *pOutBuffer++ = ch + '0';

        ch = (pInBuffer[i] >> 4) & 0x0F;
        if (ch == 0x0F)
        {
            break;
        }
        *pOutBuffer++ = ch + '0';
    }

    len = pOutBuffer - p;

    return len;
}
bool location_service_parse_response(struct am_location_service_rsp_data_t *response, uint8_t *latitude, uint8_t *longitude,
                                            uint16_t *year, uint8_t *month, uint8_t *day, uint8_t *hour, uint8_t *minute, uint8_t *second)
{
    uint8_t loc[20] = {0};
    uint32_t len = 0;
    struct am_location_service_rsp_data_t locationServiceResponse;
    if (response == NULL || latitude == NULL || longitude == NULL || year == NULL || month == NULL || day == NULL || hour == NULL || minute == NULL || second == NULL)
    {
        //LUAT_DEBUG_PRINT("location_service_parse_response: invalid parameter");
        return FALSE;
    }

    if (!(response->result == 0 || response->result == 0xFF))
    {
        //LUAT_DEBUG_PRINT("location_service_parse_response: result fail %d", response->result);
        return FALSE;
    }

    // latitude
    len = location_service_bcd_to_str(loc, response->latitude, AM_LOCATION_SERVICE_LOCATION_BCD_LEN);
    if (len <= 0)
    {
        //LUAT_DEBUG_PRINT("location_service_parse_response: latitude fail");
        return FALSE;
    }
    strncat((char *)latitude, (char *)loc, 3);
    strncat((char *)latitude, ".", 2);
    strncat((char *)latitude, (char *)(loc + 3), len - 3);
    len = location_service_bcd_to_str(loc, response->longitude, AM_LOCATION_SERVICE_LOCATION_BCD_LEN);
    if (len <= 0)
    {
       // LUAT_DEBUG_PRINT("location_service_parse_response: longitude fail");
        return FALSE;
    }
    strncat((char *)longitude, (char *)loc, 3);
    strncat((char *)longitude, (char *)".", 2);
    strncat((char *)longitude, (char *)(loc + 3), len - 3);
    *year = response->year + 2000;
    *month = response->month;
    *day = response->day;
    *hour = response->hour;
    *minute = response->minute;
    *second = response->second;

    return TRUE;
}
void get_bsloc(BASE_LOC_T *bsloc)
{

    int ret;    
    BASE_LOC_T temp_bsloc = {0};
    luat_mobile_cell_info_t cell_info;
    memset(&cell_info, 0, sizeof(cell_info));
    ret = luat_mobile_get_cell_info_async(5);
    if (ret != 0)
    {
        //LUAT_DEBUG_PRINT("cell_info_async false\r\n");
        temp_bsloc.result = BSLOC_CELL_ASYNC_FAIL;
        goto quit;
    }
    luat_rtos_task_sleep(5000);
    ret = luat_mobile_get_last_notify_cell_info(&cell_info);
    //luat_mobile_get_service_tac_or_lac(&)
    // ret = luat_mobile_get_cell_info(&cell_info);//同步方式获取cell_info
    if (ret != 0)
    {
        //LUAT_DEBUG_PRINT("get last notify cell_info false\r\n");
        temp_bsloc.result = BSLOC_GET_NOTUFY_CELL_FAIL;
        goto quit;
    }
    
    
quit:
    luat_rtos_mutex_lock(g_ebike_status.mutex, LUAT_WAIT_FOREVER);
    set_bsloc_lac(&g_dev_bsloc,cell_info.lte_service_info.tac);
    set_bsloc_mcc(&g_dev_bsloc,cell_info.lte_service_info.mcc);
    set_bsloc_mnc(&g_dev_bsloc,cell_info.lte_service_info.mnc);
    set_bsloc_rssi(&g_dev_bsloc,cell_info.lte_service_info.rssi);
    set_bsloc_cid(&g_dev_bsloc,cell_info.lte_service_info.cid);
    set_bsloc_rsrq(&g_dev_bsloc,cell_info.lte_service_info.rsrq);
    luat_rtos_mutex_unlock(g_ebike_status.mutex);

}
/**
 * 基站定位,每30秒更新一次
 */
void bsloc_task(void *param)
{
    while (get_4g_net_status(&g_dev_lte) == NET_UNCONNECT)
    {
        luat_rtos_task_sleep(1000);
       // LUAT_DEBUG_PRINT("等待网络注册");
    }
    while (1)
    {
        get_bsloc(&g_ebike_status.bsloc);
        luat_rtos_task_sleep(30000);
    }
}
/**
 * 基站定位初始化
 */
void usr_bsloc_init(void)
{
    luat_rtos_task_create(&bsloc_task_handle, 4 * 2048, 60, "bsloc_task", bsloc_task, NULL, NULL);
}

void gnss_setup_init(void)
{
    luat_uart_t uart = {
        .id = UART_ID,
        .baud_rate = 115200,
        .data_bits = 8,
        .stop_bits = 1,
        .parity = 0
        };
    //luat_uart_pre_setup(UART_ID, 1);
    luat_uart_setup(&uart);

    luat_uart_ctrl(UART_ID, LUAT_UART_SET_RECV_CALLBACK, gnss_in_uart_recv_cb);
    luat_pm_power_ctrl(LUAT_PM_POWER_GPS, 1);
}

static void gnss_in_task(void *param)
{
    while(gps_event_deal_task_handle == NULL)
    {
        usr_delay_s(1);
    }
    gnss_setup_init();
   //task_ephemeris();    
    while (1)
    {
        uint32_t id;
        gnss_data_struct *gnss = NULL;
        if (0 == luat_rtos_message_recv(gnss_in_task_handle, &id, (void **)&gnss, LUAT_WAIT_FOREVER))
        {
            libminmea_parse_data(gnss->gnss_data, gnss->gnss_data_len);
            if (gnss->gnss_data != NULL)
            {
                free(gnss->gnss_data);
                gnss->gnss_data = NULL;
            }
            if (gnss != NULL)
            {
                free(gnss);
                gnss = NULL;
            }
        }
    }
}


void usr_in_gnss_init(void)
{
    luat_rtos_task_create(&gnss_in_task_handle, 1024 * 20, 30, "gnss_in_task", gnss_in_task, NULL, 10);
}
void usr_gnss_init(void)
{
    usr_bsloc_init();
    usr_in_gnss_init();    
}

